/**
 * Tiny utility class to control ?OLED modules
 * 
 * Reference: ?OLED-96-G1 Users Manual Revision 1.0
 */

class OLED {
  public final byte SMALL_SIZE_FONT = 0;
  public final byte MEDIUM_SIZE_FONT = 1;
  public final byte LARGE_SIZE_FONT = 2;
  
  private Serial port;
  
  OLED(Serial port) {
    this.port = port;
    init();
  }
  
  public void init() {
    port.write('U');  // to set baud rate automatically
    eraseScreen();
  }
  
  public void eraseScreen() {
    byte[] data = new byte[1];
    data[0] = 0x45;
    sendCommand(data);
  }
  
  public void setBackgroundColor(float r, float g, float b) {
    int colorIn16bits = get16bitsColorFor(r, g, b);
    byte msb = (byte)((colorIn16bits >> 8) & 0xFF);
    byte lsb = (byte)(colorIn16bits & 0xFF);

    byte[] data = new byte[3];
    data[0] = 0x42;
    data[1] = msb;
    data[2] = lsb;
    sendCommand(data);
  }

  public void setPenSize(int penSize) {
    byte[] data = new byte[2];
    data[0] = 0x70;
    data[1] = (byte)penSize;
    sendCommand(data);
  }

  public void drawCircle(int cx, int cy, int radius, float r, float g, float b) {
    int colorIn16bits = get16bitsColorFor(r, g, b);
    byte msb = (byte)((colorIn16bits >> 8) & 0xFF);
    byte lsb = (byte)(colorIn16bits & 0xFF);

    byte[] data = new byte[6];
    data[0] = 0x43;
    data[1] = (byte)cx;
    data[2] = (byte)cy;
    data[3] = (byte)radius;
    data[4] = msb;
    data[5] = lsb;
    sendCommand(data);
  }

  public void drawLine(int x1, int y1, int x2, int y2, float r, float g, float b) {
    int colorIn16bits = get16bitsColorFor(r, g, b);
    byte msb = (byte)((colorIn16bits >> 8) & 0xFF);
    byte lsb = (byte)(colorIn16bits & 0xFF);

    byte[] data = new byte[7];
    data[0] = 0x4C;
    data[1] = (byte)x1;
    data[2] = (byte)y1;
    data[3] = (byte)x2;
    data[4] = (byte)y2;
    data[5] = msb;
    data[6] = lsb;
    sendCommand(data);
  }

  public void drawRectangle(int x1, int y1, int x2, int y2, float r, float g, float b) {
    int colorIn16bits = get16bitsColorFor(r, g, b);
    byte msb = (byte)((colorIn16bits >> 8) & 0xFF);
    byte lsb = (byte)(colorIn16bits & 0xFF);

    byte[] data = new byte[7];
    data[0] = 0x72;
    data[1] = (byte)x1;
    data[2] = (byte)y1;
    data[3] = (byte)x2;
    data[4] = (byte)y2;
    data[5] = msb;
    data[6] = lsb;
    sendCommand(data);
  }

  // Place String of ASCII Text (unformatted)
  public void drawText(int x, int y, byte font, float r, float g, float b, String string) {
    int colorIn16bits = get16bitsColorFor(r, g, b);
    byte msb = (byte)((colorIn16bits >> 8) & 0xFF);
    byte lsb = (byte)(colorIn16bits & 0xFF);

    int dataLength = 8 + string.length() + 1;
    byte[] data = new byte[dataLength];
    data[0] = 0x53;
    data[1] = (byte)x;
    data[2] = (byte)y;
    data[3] = font;
    data[4] = msb;
    data[5] = lsb;
    data[6] = 1;  // n * normal width
    data[7] = 1;  // m * normal height
    
    for (int i = 0; i < string.length(); i++) {
      data[8 + i] = (byte)string.charAt(i);
    }
    data[dataLength - 1] = 0x00;

    sendCommand(data);    
  }

  private void sendCommand(byte[] data) {
    port.write(data);
    while (port.available() < 1);
    int inByte = port.read();
    if (inByte != 0x06) {
      println("The command was not acknowledged");
    }
  }
  
  private int get16bitsColorFor(float r, float g, float b) {
    byte rr = (byte)(round(r * 31.0) & 0x1F);
    byte gg = (byte)(round(g * 63.0) & 0x3F);
    byte bb = (byte)(round(b * 31.0) & 0x1F);
    int result = (rr << 11) + (gg << 5) + bb;
    return result;
  }
}
